package pteam;


import java.util.*;

import simulator.Actor;
import simulator.Robot;
import simulator.Sensor;
import simulator.Simulator;
import simulator.BumpSensor;
public class PlanBActor  implements Actor {
  public final int TOLERANCE = 5;
  public final int TOOCLOSE = 20;
  public final int FARDIST = 2;
  public final int FARANGLE = 1;
  public final int NOTFARDIST = 5;
  public final int NOTFARANGLE = 1;
  public final int BACKDIST = 15;
  public final int NINETY = 90;
  public final int TOUCHINGWALL = 1000001;
  public boolean closing = false;
  public boolean wallTouch = false;
  public boolean cornering = false;
  public double prestate = 0;
  public String getMaze() {
    return "Super";
  }

  public void act(Robot r) {
    if (wallTouch) {
      Simulator.out.println("We have hit the wall!");
      r.goBackward();
      r.waitDistance(1);
      r.turnLeft();
      r.waitAngle(45);
      r.goForward();
      r.waitDistance(1);
      wallTouch = false;
    } else if (cornering) {
      r.turnRight();
      r.waitAngle(15);
      r.goForward();
      r.waitDistance(5);
    }
    else if (closing) {
          try {
           Thread.sleep(250);
           Simulator.out.println("We're close enough to the wall");
           } catch(Exception e) {}
        r.turnRight();
        r.waitAngle(FARANGLE);

    } else {
      Simulator.out.println("MOVING!");
        try { Thread.sleep(250); } catch(Exception e) {}
      r.turnLeft();
      r.waitAngle(NOTFARANGLE);
    }
    r.goForward();
    r.waitDistance(FARDIST);
  }
  public void updateSensors(Sensor[] s) {
    if (s[1].getState()[0] - prestate > TOLERANCE && prestate != 0) {
      cornering = true;
      prestate = s[1].getState()[0];
    } else if (cornering && s[1].getState()[0] < prestate) {
      cornering = false;
      prestate = s[1].getState()[0];
    }
    else if (s[1].getState()[0] > prestate) {
      closing = true;
      prestate = s[1].getState()[0];
    } else if (s[1].getState()[0] < prestate) {
      closing = false;
      prestate = s[1].getState()[0];
    } else {
      prestate = s[1].getState()[0];
    }
    
    if (s[0].getState()[0] == BumpSensor.PRESSED) {
      wallTouch = true;
      Simulator.out.println("BUMP!");
    }  
}
}